#include "individual.h"

#include <set>
#include <utility>

#include <QVector>

#include <Arete.h>
#include <Noeud.h>

static inline
std::pair<Noeud, Noeud::Sommet> otherEnd(const Arete& arete, const Noeud& node)
{
    return (arete.noeud1.id == node.id)
            ? std::make_pair<Noeud, Noeud::Sommet>(arete.noeud2, arete.position2)
            : std::make_pair<Noeud, Noeud::Sommet>(arete.noeud1, arete.position1);
}

static
btRigidBody extractConstructionInformation(const Noeud& node, MotionStateFactory* factory)
{
    //extract physic information from node
    // NOTE: so far, we assume that every part has a box shape
    btVector3 halfExtent(
                btScalar(node.width() / 2.f),
                btScalar(node.height() / 2.f),
                btScalar(node.depth() / 2.f));

    btTransform transform;
    transform.setIdentity();
    transform.setOrigin(
                btVector3(
                    btScalar(node.x()),
                    btScalar(node.y()),
                    btScalar(node.z())));

    btCollisionShape * collision_shape = new btBoxShape(halfExtent);
    //TODO: link to our custom motionstate implementation
    btMotionState *    motionstate     = factory->create(transform, halfExtent);

    btRigidBody::btRigidBodyConstructionInfo construction_information(
                node.mass(),
                motionstate,collision_shape);
    construction_information.m_friction = node.friction();
    construction_information.m_restitution = node.restitution();

    return btRigidBody(construction_information);
}

static
QVector<Arete*> connectedAretesFrom(const QList<Arete*>& aretes, const Noeud& node)
{
    QVector<Arete*> connected;
    Q_FOREACH(Arete* arete, aretes)
    {
        if( arete->noeud1.id == node.id || arete->noeud2.id == node.id )
        {
            connected.append(arete);
        }
    }
    return connected;
}

static
btVector3 connectionPoint(const Noeud::Sommet vertex, const btVector3& body_dimension)
{
	btScalar divisor(2.);
    switch(vertex)
    {
    case Noeud::A:
        return btVector3(
			  body_dimension.x() / divisor,
			- body_dimension.y() / divisor,
			- body_dimension.z() / divisor);
    case Noeud::B:				   
		return btVector3(		   
			  body_dimension.x() / divisor,
			- body_dimension.y() / divisor,
			  body_dimension.z() / divisor);
	case Noeud::C:				   
		return btVector3(		   
			- body_dimension.x() / divisor,
			- body_dimension.y() / divisor,
			  body_dimension.z() / divisor);
    case Noeud::D:				   
		return btVector3(		   
			- body_dimension.x() / divisor,
			- body_dimension.y() / divisor,
			- body_dimension.z() / divisor);
    case Noeud::E:				   
		return btVector3(		   
			  body_dimension.x() / divisor,
			  body_dimension.y() / divisor,
			- body_dimension.z() / divisor);
    case Noeud::F:				 
		return btVector3(		 
			  body_dimension.x() / divisor,
			  body_dimension.y() / divisor,
			  body_dimension.z() / divisor);
	case Noeud::G:
		return btVector3(
			- body_dimension.x() / divisor,
			  body_dimension.y() / divisor,
			  body_dimension.z() / divisor);
	case Noeud::H:
		return btVector3(
			- body_dimension.x() / divisor,
			  body_dimension.y() / divisor,
			- body_dimension.z() / divisor);
	case Noeud::Err:
	default:
		return btVector3();
    }
}

static
void recursiveIndividualConstruction(
const Graph&   graph,
std::set<int>& visited_nodes, 
MotionStateFactory* factory,
const Noeud&   current_node,
Member&        current_member)
{
    //life scope of 'connections'
    {
        QVector<Arete*> connections = connectedAretesFrom(graph.aretes(), current_node);
        Q_FOREACH(Arete* arete, connections)
        {
            std::pair<Noeud&, Noeud::Sommet> connection = otherEnd(*arete, current_node);
            const Noeud&        other_extremity = connection.first;
            const Noeud::Sommet connected_face  = connection.second;

            Member* member = new Member( extractConstructionInformation(other_extremity,factory) );

            //NOTE: simple connection as Point2Point for the moment

            // - we apply a '-' because in the root member referential,
			//this is the opposite face which is connected
			btVector3 current_dimension(current_node.width(), current_node.height(), current_node.depth());
			btVector3 other_extremity_dimension(current_node.width(), current_node.height(), current_node.depth());

            btVector3 current_connection_point =
                    connectionPoint(
                        connected_face,
                        current_dimension);
            btVector3 member_connection_point =
                    connectionPoint(
                        connected_face,
                        other_extremity_dimension);

            btTypedConstraint* constraint =
                    new btPoint2PointConstraint(
                        current_member.body(),    member->body(),
                        current_connection_point, member_connection_point);


            //now that everything is computed, let's connect parts together
            Connection member_connection(constraint, member);
			current_member.connectTo( member_connection );


			//...and recursively connect members
            if(visited_nodes.find(other_extremity.id) == visited_nodes.end()) 
			{
				visited_nodes.insert(other_extremity.id);
				recursiveIndividualConstruction(graph, visited_nodes, factory, other_extremity, *member);
			}
        }
    }
}

Individual Individual::fromGraph(const Graph &graph, MotionStateFactory* factory)
{
    Noeud& root = *graph.noeuds().at(0);

	std::set<int> visited_nodes;

    btRigidBody part_body = extractConstructionInformation(root, factory);

    Member member(part_body);

	visited_nodes.insert(root.id);
    recursiveIndividualConstruction(graph, visited_nodes, factory, root, member);

    return Individual(member);
}
